#include "polygon.h"

//#define PRINT_CONTOURS 1
//#define PRINT_POLYGON 1
//#define PRINT_RECT 1

//#define DEBUG 1

#define ABS(x) (x > 0) ? x : -x

const int MIN_AREA = 200;
const double MIN_AREA_RATIO = 0.7;

using namespace cv;
using namespace std;

void saveFrame(char *fileBase, Mat *frame)
{
    size_t fbsize, cursize;
    char buf[80];
    struct tm *tmp;
    struct timeval tv; 
    int milliseconds; 
 
    /* Obtain the time of day, and convert it to a tm struct. */ 
    gettimeofday (&tv, NULL); 

    tmp = localtime (&tv.tv_sec);
    milliseconds = tv.tv_usec / 1000;

    fbsize=strlen(fileBase);
    
    strcpy(buf, fileBase);
    
    cursize = strftime(&buf[fbsize], 80 - fbsize, "%T", tmp);
    sprintf(&buf[fbsize + cursize], ".%3.3d.jpg", milliseconds);
#ifdef DEBUG
    printf("wrote: %s\n", buf);
#endif
    imwrite(buf, *frame);
}

RectangleScanner::RectangleScanner() {}

void RectangleScanner::SetImage(Mat image){
    src = image.clone();
#if defined(SAVE_IMAGES) || defined(WINDOW)
    src_rects = image.clone();
#endif /* SAVE_IMAGES */
    rectangles.clear();
}

void RectangleScanner::PrepareImage(){

#ifdef WINDOW
    imshow("original", src);
    waitKey(0);
#endif /* WINDOW */
    
    //Preform a color threshold
    FilterColor(src);

#ifdef WINDOW
    imshow("Test1: Filter Color", src);
    waitKey(0);
#endif /* WINDOW */

    //Convert thresholded image to grayscale
    cvtColor(src, gray, CV_BGR2GRAY);

#ifdef WINDOW
    imshow("test2: Grayscale", gray);
    waitKey(0);
#endif /* WINDOW */
    
}


void RectangleScanner::FindContours(){

    //Prepare the image with a grayscale/dilation/erosion first
    PrepareImage();

    //Find contours
    findContours(gray, contours, hierarchy, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);

#ifdef DEBUG
    cout << "contours.size() = " << contours.size() << endl;
#endif

} 

void RectangleScanner::FindRectangles(){

    //First, find all the contours
    FindContours();
   
    //variables for processing 
    unsigned size = contours.size();
    bool shape_is_rectangle = false;

    for (size_t i = 0; i < (size); ++i){

#ifdef DEBUG
        cout << endl << "contours[" << i << "]:" << endl;
#endif
        //access new element 
        vector<Point> polygon = contours[i];

#ifdef PRINT_CONTOURS
        cout << "num points = " << polygon.size() << endl;
        for (size_t j=0; j< polygon.size(); j++) {
            cout << "point["<<j<<"] = " << polygon[j] << endl;
        }
#endif /* PRINT_CONTOURS */

#ifdef PRINT_POLYGON
        // We convert contours to polygons with smaller number of sides
        vector<Point> coordinates;
        /* Need to turn epsilon value */
        approxPolyDP(polygon, coordinates, 5, true);

        cout << "num points = " << coordinates.size() << endl;
        for (size_t j=0; j< coordinates.size(); j++) {
            cout << "coordinates["<<j<<"] = " << coordinates[j] << endl;
        }
#endif /* PRINT_POLYGON */

        /* minAreaRect: note it is rotated rectangle */
        RotatedRect rrect = minAreaRect(polygon);
        double polarea = contourArea(polygon);
        double rectarea = rrect.size.height*rrect.size.width;

#ifdef DEBUG
        cout << "Contour area is " << polarea << endl;
        cout << "Rect area is " << rectarea <<  endl;
#endif

        //If the shape is close in area to its smallest-area rectangle, it's a rectangle
        //If it looks like a rectangle and walks like a rectangle, it's probably a rectangle
        shape_is_rectangle = false;
        if (polarea/rectarea > MIN_AREA_RATIO) {
            shape_is_rectangle = true;

#ifdef DEBUG
            cout << "That's a rectangle, ratio is only " << polarea/rectarea << endl;
#endif

        }
        if (shape_is_rectangle){
            rectangles.push_back(rrect);
        }

        //Reset the bool after usage
        shape_is_rectangle = false;

#ifdef PRINT_RECT        
        cout << "Min Area Rectangle:" << " center = " << rrect.center;
        cout << " height = " << rrect.size.height;
        cout << " width = " << rrect.size.width;
        cout << " angle = " << rrect.angle << endl;
#endif
    }

}

void RectangleScanner::ChooseRectangle(){

    FindRectangles();

    unsigned size = rectangles.size();
    RotatedRect rrect;

    bool shape_is_large = true;
    bool shape_is_image = true;

    bool is_vertical = false;
    bool is_horizontal = false;

    int rect_count = 0;

    bool old_dynamic_on = dynamic_on;
    saw_rect = false;
    dynamic_on = false;


#ifdef DEBUG
    cout << "rectangles.size() = " << size << endl;
#endif

    for (size_t i = 0; i < (size); ++i) {
        shape_is_large = false;
        shape_is_image = false;
	
	is_vertical = false;
	is_horizontal = false;

        rrect = rectangles[i];

#ifdef PRINT_RECT        
        cout << "Min Area Rectangle:" << " center = " << rrect.center;
        cout << " height = " << rrect.size.height;
        cout << " width = " << rrect.size.width;
        cout << " angle = " << rrect.angle << endl;
#endif

        //Check that shape fulfills minimum size requirement
        if (rrect.size.height*rrect.size.width > MIN_AREA) shape_is_large = true;

        //Check if the shape is the entire image (algorithm sometimes finds it)
        if ( rrect.size.height * rrect.size.width > 290000) {
            shape_is_image = true;
        }

        if (shape_is_large && !(shape_is_image)) {
	    rect_count = rect_count +1;
	    // look at rectangle properties
            // cout << "angle = " << rrect.angle << endl;
            if(rrect.angle <=45 && rrect.angle>= -45) {
	        if(rrect.size.height > rrect.size.width){
	            is_vertical = true;
	        }
	        if(rrect.size.width > rrect.size.height){
	            is_horizontal = true;
	        }
            } else {
                if(rrect.size.height < rrect.size.width){
                    is_vertical = true;
                }
		if(rrect.size.width < rrect.size.height){
		    is_horizontal = true;
		}
            }

#ifdef DEBUG
	    if(is_vertical){
	        cout << "The rectangle is vertical" << endl;
	    } else if(is_horizontal){
	        cout << "The rectangle is horizontal" << endl;
	    } else{
	        cout << "The rectangle is a square" << endl;
	    }
#endif

	    if (is_vertical) {
#ifdef DEBUG
	        if (saw_vertical) {
		  cout << "SAW MORE THEN ONE VERTICAL RECTANGLE" << endl;
		}
#endif
	        saw_vertical = true;
	    }

	    if (is_horizontal) {
#ifdef DEBUG
	        if (saw_horizontal) {
		  cout << "SAW MORE THEN ONE HORIZONTAL RECTANGLE" << endl;
		}
#endif
	        saw_horizontal = true;
	    }


#if defined(SAVE_IMAGES) || defined(WINDOW)
	    Point2f vertices[4];
	    rrect.points(vertices);
	    for (int j = 0; j < 4; j++){
	        line(src_rects, vertices[j], vertices[(j+1)%4], Scalar(0,0,255));
            }
            
	    circle(src_rects, rrect.center, 5, Scalar(255,0,0));
#endif /* defined(SAVE_IMAGES) || defined(WINDOW) */
	}
    }

    if ((rect_count >= 1) && saw_vertical) {
        saw_rect = true;
	if(rect_count > 1 && saw_horizontal) {
	     dynamic_on = true;
	}
    }
    
    if (old_dynamic_on != dynamic_on) {
#ifdef TRANSITION_DEBUG
	cout << "dynamic_on = " << dynamic_on << endl;
#endif
    }

#ifdef DEBUG
    if(saw_rect){
      cout << "Saw a rectangle"<< endl;
      if(dynamic_on){
	cout << "The dynamic target is on" << endl;
      }else{
	cout << "The dynamic target is off" << endl;
      }
    } else {
       cout << "Did not see a rectangle"<< endl;
    }     
#endif    
#if WINDOW
    imshow("rectangle", src_rects);
    waitKey(0);
    destroyAllWindows();
#endif
}

